#include "emac_internal.h"
#include "emac.h"
#include "semaphores.h"
#include "os_mem.h"
#include "os_io.h"
#include "sw_interrupt.h"
#include "dev.h"
#include "sockets.h"
#include "arp.h"
#include "emac_params.h"
#include "sh_mem.h"

#ifdef NO_VM
static emac_rx_ctx_t rx_ctx;
#else
#error "rx_ctx is shared b/w process and ISR"
#endif

static emac_params_t *emac_params = NULL;
static arp_table_t *arp_table = NULL;

static void free_rx_descriptors(void)
{
    uint32_t i = 0;
    pkt_desc_t *desc = NULL;

    for (i = 0; i < NUM_RX_BUFFERS; i++)
    {
	desc = (pkt_desc_t *)(RX_DESC_MEM_BASE) + i;
	free(desc->buf_ptr);
    }

    return;
}

static err_t init_rx_descriptors(void)
{
    uint32_t i = 0;
    uint32_t j = 0;
    pkt_desc_t *desc_ptr = NULL;
    uint8_t *pkt_ptr = NULL;

    for (i = 0; i < NUM_RX_BUFFERS; i++)
    {
	desc_ptr = (pkt_desc_t *)(RX_DESC_MEM_BASE) + i;
	pkt_ptr = malloc(BUFFER_LEN);   // Needs to come from shared memory
	if (!pkt_ptr) {
	    printf("Short of memory while "
		    "initializing RX buffers: 0x%x\n", i);

	    for (j = 0; j < i; j++) {
		free(((pkt_desc_t *)RX_DESC_MEM_BASE)[j].buf_ptr);
	    }
	    return ERR_NO_MEM;
	}

	if (i != NUM_RX_BUFFERS - 1) {
	    desc_ptr->next_ptr = (desc_ptr + 1);
	} else {
	    desc_ptr->next_ptr = (pkt_desc_t *)RX_DESC_MEM_BASE;
	}

	desc_ptr->buf_ptr = pkt_ptr;
	desc_ptr->buf_len = BUFFER_LEN;
	desc_ptr->flags = EMAC_DSC_FLAG_OWNER;  // belongs to EMAC
    }

    return ERR_SUCCESS;

}

static void register_emac_rx(emac_cfg_t *emac_cfg)
{
    raise_sw_interrupt(SW_INT_EMAC_CFG, (uint32_t)emac_cfg);
    return;
}

static void replenish_rx_ring(void)
{

    uint16_t fill_head = rx_ctx.buffer_fill_head;
    pkt_desc_t *desc_ptr;

    while (rx_ctx.buffers_consumed) {

	uint8_t *ptr = malloc(BUFFER_LEN);
	if (!ptr) {
	    printf("Insufficient memory while repleneshing rx ring\n");
	    break;
	}

	desc_ptr = (pkt_desc_t *)RX_DESC_MEM_BASE + fill_head;
	desc_ptr->buf_ptr = ptr;
	desc_ptr->buf_len = BUFFER_LEN;
	desc_ptr->buf_offset = 0;
	desc_ptr->flags = EMAC_DSC_FLAG_OWNER; // submit to EMAC module
	rx_ctx.buffers_consumed--;
	RX0FREEBUFFER_REG = 1;

	fill_head = (fill_head + 1) & (NUM_RX_BUFFERS - 1);
    }

    rx_ctx.buffer_fill_head = fill_head;
}

static void update_arp_table(uint32_t ip_addr, uint8_t *mac_addr)
{
    uint16_t i = 0;
    arp_entry_t *arp_entry = NULL;
    gather_list_t *gather_list = NULL;

    for (i = 0; i < MAX_ARP_ENTRIES; i++) {
        if (arp_table->entries[i].ip_address == ip_addr && arp_table->entries[i].mac_addr_present == false) {
	    
	    arp_entry = &arp_table->entries[i];
	    sem_lock(&arp_entry->entry_sem);
	    if (arp_entry->ip_address == ip_addr) {
	        // Entry is still there after sem lock
		gather_list = (gather_list_t *)calloc(1, sizeof(gather_list_t));
		if (!gather_list) {
		    printf("Gather list allocation failed\n");
		    sem_unlock(&arp_entry->entry_sem);
		    return;
		}

		gather_list->pending_pkt_flush = true;
		memcpy(arp_entry->mac_addr, mac_addr, 6);
		arp_entry->mac_addr_present = true;
		arp_entry->entry_recently_updated = true;
		gather_list->num_entries = i;  // parameter overloaded to specify arp table index

		// trigger emac-tx to send pending packets
		//printf("Sending pending packet queue flush for ip: 0x%x to TX task on queue:0x%x\n", ip_addr, emac_params->tx_queue_id);
		enqueue_shared_data(emac_params->tx_queue_id, (uint8_t *)gather_list, sizeof(gather_list_t));
		sem_unlock(&arp_entry->entry_sem);
		break;
            }
        }
    }

    return;
}

static inline 
void send_arp_response(arp_pkt_t *arp_pkt)
{
    gather_list_t *gather_list = NULL;

    //printf("ARP request received. Sending response\n");

    memcpy(arp_pkt->dst_mac, arp_pkt->src_mac, 6);
    memcpy(arp_pkt->src_mac, emac_params->mac_addr, 6);
    arp_pkt->opcode = htons(2);  // arp reply

    arp_pkt->target_ip = arp_pkt->sender_ip;
    memcpy(arp_pkt->target_mac, arp_pkt->sender_mac, 6);

    arp_pkt->sender_ip = htonl(emac_params->ip_addr);
    memcpy(arp_pkt->sender_mac, emac_params->mac_addr, 6);

    gather_list = (gather_list_t *)calloc(1, sizeof(gather_list_t));
    if (!gather_list) {
        printf("Could not build ARP response. calloc failed.\n");
	free(arp_pkt);
	return;
    }

    gather_list->num_entries = 1;
    gather_list->buf_list[0].buf_ptr = (uint8_t *)arp_pkt;
    gather_list->buf_list[0].len = sizeof(arp_pkt_t);
    enqueue_shared_data(emac_params->tx_queue_id, (uint8_t *)gather_list, sizeof(gather_list_t));

    return;
}
   
static inline
bool not_for_us_broadcast(uint8_t *pkt_ptr)
{
    ip_hdr_t *ip_hdr = (ip_hdr_t *)pkt_ptr;
    uint8_t i = 0;
    
    for (i = 0; i < 6; i++) {
       if (ip_hdr->dst_mac[i] != 0xFF) {
           break;
       }
    }

    if (i != 6) {
        // not broadcast
	return false;
    }

    if (ip_hdr->dest_ip != htonl(emac_params->ip_addr)) {
        //printf("Received broadcast, but not for us. Ignored\n");
        return true;
    } else {
        return false;
    }

}


static inline
bool handle_arp_resolution(uint8_t *pkt_ptr)
{
    ip_hdr_t *ip_hdr = (ip_hdr_t *)pkt_ptr;
    arp_pkt_t *arp_pkt = (arp_pkt_t *)pkt_ptr;

    //printf("Received packet of ether proto: 0x%x\n", ntohs(arp_pkt->eth_proto));

    if (ntohs(arp_pkt->eth_proto) == 0x0806) {
        // Its an arp packet
	update_arp_table(ntohl(arp_pkt->sender_ip), (uint8_t *)arp_pkt->sender_mac);

	// If its an arp request, then send an arp response
	if (ntohs(arp_pkt->opcode) == 0x01 && ntohl(arp_pkt->target_ip) == emac_params->ip_addr) {
	    send_arp_response(arp_pkt);
	} else {
	    //printf("Received arp packet but does not require further handling. Dropped\n");
	    free(pkt_ptr);  // arp packets are not processed further
	}
	return true;
    } else if (ntohs(arp_pkt->eth_proto) == 0x0800) {
        // Its an IP packet
	update_arp_table(ntohl(ip_hdr->source_ip), (uint8_t *)ip_hdr->src_mac);
	return false;
    } else {
        // Non IP packets not handled currently
	//printf("Received non IP/ARP packet. Dropped\n");
	free(pkt_ptr);
	return true;
    }
}

void process_packet(uint8_t *pkt_ptr, uint16_t len)
{
   dev_post_msg_t post_msg;
   udp_hdr_t *udp_hdr = NULL;

   if (handle_arp_resolution(pkt_ptr)) {
       // Arp-request, need not be sent further
       return;
   }

   if (not_for_us_broadcast(pkt_ptr)) {
       // Its an L2 broadcast but dest IP is not ours
       free(pkt_ptr);
       return;
   }
   
   post_msg.dev_info.name = NULL;
   post_msg.dev_info.type = DEVICE_TYPE_NETWORK_SOCKET;
   post_msg.dev_info.size = 0; // no used

   udp_hdr = (udp_hdr_t *)pkt_ptr;
   if (udp_hdr->ip_hdr.l4_proto == 0x11) {
       // UDP packet
       post_msg.dev_info.address = udp_hdr->dst_port;
       post_msg.dev_info.subtype = DEVICE_SUBTYPE_UDP_SOCKET; 
   } else {
       post_msg.dev_info.address = udp_hdr->ip_hdr.l4_proto;
       post_msg.dev_info.subtype = DEVICE_SUBTYPE_IP_SOCKET;
   }

   post_msg.msg.msg_data = pkt_ptr;
   post_msg.msg.msg_len = len;
   post_msg.msg.data_type = SHARED_DATA;
   post_msg.msg.msg_id = Q_MSG_TYPE_DEVICE_DATA;
   post_msg.msg.err_code = ERR_SUCCESS;

   raise_sw_interrupt(SW_INT_DEVICE_POST, (uint32_t)&post_msg);

   if (post_msg.msg.err_code != ERR_SUCCESS) {
       free(pkt_ptr);
   }
   
   return;
}

static void start_rx(sem_t *emac_rx_sem)
{

    uint16_t head = 0;
    uint16_t tail = 0;
    pkt_desc_t *desc_ptr = NULL;

    while (1) {

	sem_lock(emac_rx_sem);
	
	//printf("RX: Received something with head:0x%x, tail:0x%x\n", 
	 //head, tail);

	head = rx_ctx.head;
	tail = rx_ctx.tail;

	while (head != tail) {

	    desc_ptr = (pkt_desc_t *)RX_DESC_MEM_BASE + head;
	    head = (head + 1) & (NUM_RX_BUFFERS - 1);
	    rx_ctx.buffers_consumed++;

	    if (RX_BUFFER_ERR(desc_ptr)) {
		printf("RX buffer error\n");
		free(desc_ptr->buf_ptr);
		tail = rx_ctx.tail;
		continue;
	    }

	    if (!SOP_BUFFER(desc_ptr) || !EOP_BUFFER(desc_ptr)) {
		// Buffer chaining not supported yet
		printf("SOP/EOP error in buffer!!\n");
		free(desc_ptr->buf_ptr);
		tail = rx_ctx.tail;
		continue;
	    }

	    if (desc_ptr->packet_len > BUFFER_LEN) {
	        // oversized frame!!
		printf("Oversized frame received\n");
		free(desc_ptr->buf_ptr);
		tail = rx_ctx.tail;
		continue;
	    }

	    // valid packet received
	    process_packet(desc_ptr->buf_ptr, desc_ptr->packet_len);
	    tail = rx_ctx.tail;

	}

	rx_ctx.head = head;  // advance the head

	// replenish the descriptor ring
	replenish_rx_ring();
    }
}


void emac_rxfun(void)
{
    sem_t *emac_rx_sem = NULL;
    err_t sem_error = ERR_SUCCESS;
    emac_cfg_t emac_cfg;

    void *handle = NULL;
    err_t err_code = ERR_SUCCESS;

    err_t ret_val = ERR_SUCCESS;
    ret_val = init_rx_descriptors();
    if (ret_val != ERR_SUCCESS) {
	return;
    }

    sem_error = sem_create_shared(&emac_rx_sem);
    if (sem_error != ERR_SUCCESS) {
	printf("Error in creating shared semaphore\n");
	free_rx_descriptors();
	return;
    }

    // Ensure the semaphore is locked before first use
    sem_trylock(emac_rx_sem);

    rx_ctx.head = 0;
    rx_ctx.tail = 0;
    rx_ctx.buffer_fill_head = 0;
    rx_ctx.buffers_consumed = 0;

    memset((uint8_t *)&emac_cfg, 0, sizeof(emac_cfg_t));
    emac_cfg.emac_rx_sem = emac_rx_sem;
    emac_cfg.rx_ctx = &rx_ctx;
    register_emac_rx(&emac_cfg);

    RX0HDP_REG = RX_DESC_MEM_BASE;
    //printf("Initializing channel 0 HDP with address: 0x%x\n", RX_DESC_MEM_BASE);

    /*
     * Now we are ready to activate the interrupts
     */
    RXCONTROL_REG = 1;

    // Initialized in TX
    //MACCONTROL_REG = MACCONTROL_REG | (0x1 << 5);  // enable GMIIEN

    // Enable RX module interrupts
    C0RXTHRESHEN_REG = 1;
    //C1RXTHRESHEN_REG = 1;
    C0RXEN_REG = 1;
    //C1RXEN_REG = 1;
    //C0MISCEN_REG = 0x4;  => Initialized in TX already
    //C1MISCEN_REG = 0x4;

    // Get handle to emac-tx params and arp-table
    err_code = open_shared_memory(EMAC_PARAMS_MEM, &handle, (void **)&emac_params);
    if (err_code != ERR_SUCCESS) {
        printf("RX task unable top open %s memory.\n", EMAC_PARAMS_MEM);
	return;
    }

    err_code = open_shared_memory(ARP_TABLE_MEM, &handle, (void **)&arp_table);
    if (err_code != ERR_SUCCESS) {
        printf("RX task unable top open %s memory.\n", ARP_TABLE_MEM);
	return;
    }

    start_rx(emac_rx_sem);

}
